/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "Wrench.h"

namespace RobotPlant { namespace Math{
	Wrench::Wrench(void):Force(),Moment()
	{
	}

	Wrench::Wrench(const Wrench& arg)
	{
		Force = arg.Force;
		Moment = arg.Force;
	}

	Wrench::Wrench(const Vector3& force,const Vector3& torque):Force(force),Moment(torque)
	{

	}

	Wrench::~Wrench(void)
	{
	}

	double& Wrench::operator[](const int index)
	{
		if( (index < 0) || (index > 5) )
		{
			throw(new std::exception("Argument row must be between 0 and 5."));
		}

		if (index < 3) 
			return Force[index];
		else
			return Moment[index - 3];
	}

	double Wrench::operator[](const int index) const
	{
		if( (index < 0) || (index > 5) )
		{
			throw(new std::exception("Argument row must be between 0 and 5."));
		}

		if (index < 3) 
			return Force[index];
		else
			return Moment[index - 3];
	}

	Wrench& Wrench::operator = (const Wrench& arg)
	{
		Force = arg.Force;
		Moment = arg.Moment;
		return *this;
	}

	Wrench& Wrench::operator += (const Wrench& arg)
	{ 
		Moment += arg.Moment;
		Force +=arg.Force;
		return *this;
	}

	Wrench& Wrench::operator -= (const Wrench& arg)
	{
		Moment -= arg.Moment;
		Force -= arg.Force;
		return *this;
	}

	bool operator == ( const Wrench& lhs,const Wrench& rhs )
	{
		return Equal(lhs,rhs);
	}

	bool operator != ( const Wrench& lhs,const Wrench& rhs ) 
	{
		return !Equal(lhs,rhs);
	}

	// addition of Wrench's
	Wrench operator + ( const Wrench& lhs,const Wrench& rhs )
	{
		return Wrench(lhs.Force + rhs.Force, lhs.Moment + rhs.Moment);
	}

	Wrench operator - ( const Wrench& lhs,const Wrench& rhs )
	{
		return Wrench(lhs.Force - rhs.Force, lhs.Moment - rhs.Moment);
	}

	Wrench operator - ( const Wrench& arg )
	{
		return Wrench( -arg.Force, -arg.Moment );
	}

	Wrench operator * ( const Wrench& lhs, const double rhs )
	{
		return Wrench(lhs.Force * rhs, lhs.Moment * rhs);
	}

	Wrench operator * ( const double lhs,const Wrench& rhs )
	{
		return Wrench(lhs * rhs.Force,lhs * rhs.Moment);
	}

	double operator * (const Wrench& lhs,const Velocity& rhs) 
	{
		return rhs.LinearVel * lhs.Force + rhs.AngleVel * lhs.Moment;
	}

	Wrench operator / ( const Wrench& lhs, const double rhs )
	{
		return Wrench(lhs.Force/rhs, lhs.Moment/rhs);
	}

	void Wrench::SetZero() 
	{
		Force.SetZero();
		Moment.SetZero();
	}

	void Wrench::ReverseSign()
	{   
		Moment.ReverseSign();
		Force.ReverseSign();
	}

	Wrench Wrench::RefPoint(const Vector3& v_base_AB) const
	{
		return Wrench(Force, Moment + CrossProd(Force, v_base_AB ));
	}

	Wrench Wrench::RefBaseFrame ( const Frame3& old_base_in_new_base) const
	{
		return Wrench(old_base_in_new_base.Rotation * Force, old_base_in_new_base.Rotation * Moment);
	}

	Wrench Wrench::Zero()
	{
		return Wrench(Vector3::Zero(),Vector3::Zero());
	}


	bool Equal(const Wrench& a,const Wrench& b,const double eps)
	{
		return (Equal(a.Force,b.Force,eps) && Equal(a.Moment,b.Moment,eps)  );
	}

	Wrench AddDelta(const Wrench& a,const Wrench& da,const double dt)
	{
		return Wrench(AddDelta(a.Force,da.Force,dt),AddDelta(a.Moment,da.Moment,dt));
	}

	Wrench Diff(const Wrench& a,const Wrench& b,const double dt) 
	{
		return Wrench(
			Diff(a.Force,b.Force,dt),
			Diff(a.Moment,b.Moment,dt)
			);
	}

	void Random(Wrench& arg) 
	{
		Random(arg.Moment);
		Random(arg.Force);
	}

	void PosRandom(Wrench& arg) 
	{
		PosRandom(arg.Moment);
		PosRandom(arg.Force);
	}
}}
